/***************************************************************************
 创建者: 华磊
 开始时间: 2018.9.26
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "linematch.h"
#include <qdebug.h>
#include <QQueue>

using namespace std;
#define D_SEGMENT_MIN_SIZE 30
#define D_MATCH_DEBUG 0

LineMatch::LineMatch()
{
    minumIntensity=0;
    maxLaserRange=1000.0;
    minAngle=-10*M_PI;
    maxAngle=10*M_PI;
    isPrintVmarkProcessData=false;
    lineFittingMethod=1;
}

int LineMatch::setLineFittingMethod(int methodIn)
{
    lineFittingMethod=methodIn;
    qDebug()<<"setLineFittingMethod="<<lineFittingMethod;
    return 1;
}

int LineMatch::setIsPrintVmarkProcessData(bool isPrint)
{
    isPrintVmarkProcessData=isPrint;
    return 1;
}

int LineMatch::setLaserValidRegion(std::vector<double> region_x, std::vector<double> region_y)
{
    laserRegion_x=region_x;
    laserRegion_y=region_y;
    return 1;
}

int LineMatch::setLaserIntensityFilter(double minumIntensityIn, double maxLaserRangeIn, double minAngleIn, double maxAngleIn)
{
    minumIntensity=minumIntensityIn;
    maxLaserRange=maxLaserRangeIn;
//    minAngle=minAngleIn;
//    maxAngle=maxAngleIn;屏蔽掉，傲视雷达角度超过３６０度
    qDebug()<<"LineMatch setLaserIntensityFilter"<<"minumIntensity"<<minumIntensity<<"maxLaserRange"
           <<maxLaserRange<<"minAngle"<<minAngle<<"maxAngle"<<maxAngle;
    return 1;
}

int LineMatch::detectTwoLine(StructLineParameter lineParameter, const LaserDataInfo &laserData,
                             vector<double> &x_out, vector<double> &y_out, vector<double> &r_out,
                             int &result_line1, int &result_line2,
                             double &gapDistanceOut, double &angleBiasOut,
                             double &line1_corelation,double&line2_corelation)
{
    laser_angleIncrement=laserData.angle_increment;
    //v槽边角很容易出现与激光射线在同一直线上的多个错误点（透明胶、反射贴、高度镜面）
    //firstLineMaxLength 比实际ｖ槽单边长度最少小５ｃｍ, 这５ｃｍ用来判别ｖ槽中心。
    //情况１，预留５ｃｍ边角，但是检查到达５ｃｍ了，还是没有出现夹角减小，一直到了ｖ槽边上的几个错误数据，把ｖ槽夹角加大了。解决方法，更换没有错误数据的材料。
    //或者加大预留边长度。allowMaxDeltaDistance减小，来过滤跳点。但是考虑到探测范围很远的时候，正常的点间距也会比较大。

    vector<MathPoint> line1Start_out, line1End_out;
    vector<MathPoint> line2Start_out, line2End_out;

    int tmpKey=detectTwoReflectLine_withFitting(lineParameter.minAngle,lineParameter.maxAngle,
                                  lineParameter.firstLineMinLength,lineParameter.firstLineMaxLength,
                         lineParameter.allowMaxDeltaDistance,laserData,
                         line1Start_out, line1End_out,line2Start_out, line2End_out,
                         line1_corelation,line2_corelation,x_out,y_out,r_out ,angleBiasOut);
    result_line1=x_out.size();
    return tmpKey;


}

//int LineMatch::detectTwoLine_old(StructLineParameter lineParameter, const LaserDataInfo &laserData,
//                             vector<double> &x_out, vector<double> &y_out, vector<double> &r_out, int &result_line1, int &result_line2,
//                             double &gapDistanceOut, double &angleBiasOut,double &line1_corelation,double&line2_corelation)
//{
//    //v槽边角很容易出现与激光射线在同一直线上的多个错误点（透明胶、反射贴、高度镜面）
//    //firstLineMaxLength 比实际ｖ槽单边长度最少小５ｃｍ, 这５ｃｍ用来判别ｖ槽中心。
//    //情况１，预留５ｃｍ边角，但是检查到达５ｃｍ了，还是没有出现夹角减小，一直到了ｖ槽边上的几个错误数据，把ｖ槽夹角加大了。解决方法，更换没有错误数据的材料。
//    //或者加大预留边长度。allowMaxDeltaDistance减小，来过滤跳点。但是考虑到探测范围很远的时候，正常的点间距也会比较大。

//    vector<MathPoint> line1Start_out, line1End_out;
//    vector<MathPoint> line2Start_out, line2End_out;

//    int tmpResult=detectTwoReflectLine_withFitting(lineParameter.minAngle,lineParameter.maxAngle,
//                                  lineParameter.firstLineMinLength,lineParameter.firstLineMaxLength,
//                         lineParameter.allowMaxDeltaDistance,laserData,line1Start_out, line1End_out,line2Start_out, line2End_out,
//                                                line1_corelation,line2_corelation );
//    if(1==tmpResult)
//    {
//        result_line1=line1Start_out.size();
//        result_line2=line2Start_out.size();
//        x_out.resize(result_line1);
//        y_out.resize(result_line1);
//        r_out.resize(result_line1);
//    }
//    else
//    {
//        result_line1=0;
//        result_line2=0;
//    }
//    if(result_line1>1)
//    {
//        if(isPrintVmarkProcessData)
//        {
//             qDebug()<<"error1002 result_line1>1   line1="<<result_line1;
//        }
//        return -1;
//    }

//    if(result_line1==0)
//    {
//        if(isPrintVmarkProcessData)
//        {
//             qDebug()<<"error1003 result_line1==0";
//        }
//        return -1;
//    }

//    if(result_line2==0)
//    {
//        if(isPrintVmarkProcessData)
//        {
//             qDebug()<<"error1004 result_line2==0";
//        }
//        return -2;
//    }

//    std::vector<double> tmp_angleBiasOut;
//    std::vector<double> tmp_gapDistanceOut;
//    detectStructLine_vmark(line1Start_out, line1End_out,line2Start_out, line2End_out,lineParameter.minGap,lineParameter.maxGap,
//                         lineParameter.minAngle,lineParameter.maxAngle,x_out,y_out,r_out,tmp_gapDistanceOut,tmp_angleBiasOut);
//    if(tmp_gapDistanceOut.size()>0)
//    {
//        gapDistanceOut=tmp_gapDistanceOut[0];
//    }
//    else
//    {
//        if(isPrintVmarkProcessData)
//        {
//             qDebug()<<"error1005 没有探测到Ｖ槽";
//        }
//    }
//    if(tmp_angleBiasOut.size()>0)
//    {
//        angleBiasOut=tmp_angleBiasOut[0];
//    }
//    return 1;

//}


int LineMatch::detectReflectLine_old(double minLength,double maxLength,double allowMaxDeltaDistance,const LaserDataInfo &laserData,
                                 vector<double> &x_out, vector<double> &y_out, vector<double> &r_out)
{
    double angleBias;
    int dotCount=laserData.ranges.size();
    double deltaDistance=100000;
    double tmp_angle;
    double compare_angle;
    double tmp_x1,tmp_y1;
    double tmp_x2,tmp_y2;
    double tmp_x_firstPoint,tmp_y_firstPoint;
    double tmp_x_endPoint,tmp_y_endPoint;
    double segmentLength;

    QVector<QVector<double> > detectedReflectLines;

    int okPointCount=0;
    int goodSegmentCount=0;
    bool isPointChange;//点的数据是否有变化
    QVector<double> tmpPointList_x;//用于拟合
    QVector<double> tmpPointList_y;

    QVector<double> tmpLine;
    QVector<int> pointIndex;
    QVector<double> startPointList_x;
    QVector<double> startPointList_y;
    QVector<double> endPointList_x;
    QVector<double> endPointList_y;
    for(int i=0;i<dotCount;i++)
    {
        if(i==dotCount-1)
        {
//            qDebug()<<"last";
        }
//        if(laserData.ranges[i]>laserData.range_max && i!=dotCount-1)
//        {
//            continue;
//        }
        if(false==isLaserPointOk_laserFilter(laserData,i) && i!=dotCount-1)
        {
            continue;
        }


        if(0==i)
        {
            tmp_x1=laserData.ranges[i]*cos(laserData.angle_min+laserData.angle_increment*i);
            tmp_y1=laserData.ranges[i]*sin(laserData.angle_min+laserData.angle_increment*i);

            break;
        }
        else if(1==i)
        {
            tmp_x2=laserData.ranges[i]*cos(laserData.angle_min+laserData.angle_increment*i);
            tmp_y2=laserData.ranges[i]*sin(laserData.angle_min+laserData.angle_increment*i);


        }
        else if( laserData.ranges[i]<=laserData.range_max)
        {
            tmp_x1=tmp_x2;
            tmp_y1=tmp_y2;
            tmp_x2=laserData.ranges[i]*cos(laserData.angle_min+laserData.angle_increment*i);
            tmp_y2=laserData.ranges[i]*sin(laserData.angle_min+laserData.angle_increment*i);
            isPointChange=true;
        }
        //覆盖以前的点
        if(0==okPointCount)
        {
            tmp_x_firstPoint=tmp_x1;
            tmp_y_firstPoint=tmp_y1;
        }

        deltaDistance=sqrt((tmp_y1-tmp_y2)*(tmp_y1-tmp_y2)+(tmp_x1-tmp_x2)*(tmp_x1-tmp_x2));
//        tmp_angle=atan2(tmp_y2-tmp_y1,tmp_x2-tmp_x1);
        if(deltaDistance<allowMaxDeltaDistance && i!=dotCount-1)
        {
            if(0==okPointCount)
            {
//                compare_angle=tmp_angle;
                tmp_x_firstPoint=tmp_x1;
                tmp_y_firstPoint=tmp_y1;
                tmpPointList_x.append(tmp_x2);
                tmpPointList_y.append(tmp_y2);
            }
            tmpPointList_x.append(tmp_x1);
            tmpPointList_y.append(tmp_y1);
            okPointCount++;

        }
        else
        {
            if(true==isPointChange)
            {
                tmp_x_endPoint=tmp_x1;
                tmp_y_endPoint=tmp_y1;
                segmentLength=sqrt((tmp_y_firstPoint-tmp_y_endPoint)*(tmp_y_firstPoint-tmp_y_endPoint)+
                                   (tmp_x_firstPoint-tmp_x_endPoint)*(tmp_x_firstPoint-tmp_x_endPoint));
                if(segmentLength<maxLength && segmentLength>minLength)
                {
                    goodSegmentCount++;
                    //拟合直线
    //                myFitLine(tmpPointList_x,tmpPointList_y,tmpLine);//todo 怎么样区分拐点
                    detectedReflectLines.append(tmpLine);
                    tmpPointList_x.clear();
                    tmpPointList_y.clear();
                    pointIndex.append(i);
                    startPointList_x.append(tmp_x_firstPoint);
                    startPointList_y.append(tmp_y_firstPoint);
                    endPointList_x.append(tmp_x_endPoint);
                    endPointList_y.append(tmp_y_endPoint);
                }
                okPointCount=0;
                isPointChange=false;
            }

        }
    }

//    if(goodSegmentCount==0)
//    {
//        qDebug()<<"no good segment!goodSegmentCount="<<goodSegmentCount;
//        return -1;
//    }
//    else if(goodSegmentCount>1)
//    {
//        qDebug()<<"too many good segment!goodSegmentCount="<<goodSegmentCount<<"goodIndex="<<pointIndex
//               <<"startPointList_x="<<startPointList_x<<"startPointList_y="<<startPointList_y
//                 <<"endPointList_x="<<endPointList_x<<"endPointList_y="<<endPointList_y;
//        return -2;
//    }


    //计算坐标系
    int segmentSize=startPointList_x.size();
    x_out.resize(segmentSize);
    y_out.resize(segmentSize);
    r_out.resize(segmentSize);
    for(int i=0;i<segmentSize;i++)
    {

        x_out[i]=(startPointList_x[i]+endPointList_x[i])/2.0;
        y_out[i]=(startPointList_y[i]+endPointList_y[i])/2.0;
        r_out[i]=atan2(endPointList_y[i]-startPointList_y[i],endPointList_x[i]-startPointList_x[i]);
    }



    return 1;
}

int LineMatch::detectReflectLine_withFitting_old(int segmentJudgeMethod,double allowIntensityBias,double minLength, double maxLength,
                                             double allowMaxDeltaDistance, const LaserDataInfo &laserData,
                                 vector<MathPoint> &lineStart_out, vector<MathPoint> &lineEnd_out)
{
    qDebug()<<"LineMatch::detectReflectLine_withFitting begin";
    double standardCorelation=0.85;//偶尔出现0.847
    double breakCorelation=0.05;
    int dotCount=laserData.ranges.size();
    double deltaDistance=100000;
    double deltaIntensity=0;
//    double tmp_first_angle;
//    double tmp_second_angle;
    double allowAngleBias=30.0/180.0*M_PI;
//    double allowIntensityBias=30;
//    bool isAngleCompare=false;
    MathPoint tmp_p1;//第一个样点
    double intensity_p1;
    MathPoint tmp_p2;//第二个样点
    double intensity_p2;
    MathPoint tmp_firstPoint;//直线段起始点
    MathPoint tmp_endPoint;//直线段结束点
    double segmentLength;//单段直线长度
    QQueue<MathPoint > miniSegmentQueue;//用于计算拐点

    int okPointCount=0;
    int goodSegmentCount=0;
    bool isPointChange;//点的数据是否有变化
    QVector<QQueue<MathPoint> > tmpPointArry;//用于拟合
    QQueue<MathPoint>  tmpPointListForFit;//用于拟合
    QVector<int> pointIndex;

    for(int i=0;i<dotCount;i++)
    {
        if(i==dotCount-1)
        {
//            qDebug()<<"last";
        }
        if(false==isLaserPointOk_laserFilter(laserData,i) && i!=dotCount-1)
        {
            continue;
        }

        if(0==i)
        {
            tmp_p1.x=laserData.ranges[i]*cos(laserData.angle_min+laserData.angle_increment*i);
            tmp_p1.y=laserData.ranges[i]*sin(laserData.angle_min+laserData.angle_increment*i);
            intensity_p1=laserData.intensities[i];
            break;
        }
        else if(1==i)
        {
            tmp_p2.x=laserData.ranges[i]*cos(laserData.angle_min+laserData.angle_increment*i);
            tmp_p2.y=laserData.ranges[i]*sin(laserData.angle_min+laserData.angle_increment*i);
            intensity_p2=laserData.intensities[i];


        }
        else if( laserData.ranges[i]<=laserData.range_max)
        {
            tmp_p1=tmp_p2;
            intensity_p1=intensity_p2;
            tmp_p2.x=laserData.ranges[i]*cos(laserData.angle_min+laserData.angle_increment*i);
            tmp_p2.y=laserData.ranges[i]*sin(laserData.angle_min+laserData.angle_increment*i);
            intensity_p2=laserData.intensities[i];
            miniSegmentQueue.enqueue(tmp_p2);
            if(miniSegmentQueue.size()>D_SEGMENT_MIN_SIZE)
            {
                miniSegmentQueue.dequeue();
            }
            isPointChange=true;
        }
        //覆盖以前的点
        if(0==okPointCount)
        {
            tmp_firstPoint=tmp_p1;
        }

        deltaDistance=sqrt((tmp_p1.y-tmp_p2.y)*(tmp_p1.y-tmp_p2.y)+(tmp_p1.x-tmp_p2.x)*(tmp_p1.x-tmp_p2.x));
        deltaIntensity=fabs(intensity_p2-intensity_p1);
        bool isSegmentEnd;
        bool isPointDistanceSkip;
        if(deltaDistance<allowMaxDeltaDistance)
        {
            isPointDistanceSkip=false;
        }
        else
        {
            isPointDistanceSkip=true;
            qDebug()<<"isPointSkip=true";
        }

        if(0==segmentJudgeMethod)
        {
            isSegmentEnd=isPointDistanceSkip;
        }
        else if(1==segmentJudgeMethod)
        {
            bool isLineAngleSKip;//斜率变换判断
            double tmp_angleBias=0;
            bool is_line_fit =getFitLineAngleBias(miniSegmentQueue,breakCorelation,tmp_angleBias);
            if( tmp_angleBias> allowAngleBias
                    || false == is_line_fit)
            {
                qDebug()<<"isBreakPointResult=true,is_line_fit="<<is_line_fit<<", allowAngleBias"<<allowAngleBias*180.0/M_PI
                         <<", tmp_angleBias"<<tmp_angleBias*180.0/M_PI;
                isLineAngleSKip=true;
            }
            else
            {
                isLineAngleSKip=false;
            }
            isSegmentEnd=isLineAngleSKip | isPointDistanceSkip;

        }
        else if(3==segmentJudgeMethod)
        {
            bool isPointIntensitySKip;//光点强度变换判断
            if(deltaIntensity>allowIntensityBias)
            {
                qDebug()<<"deltaIntensity exceed"<<deltaIntensity;
                isPointIntensitySKip=true;
            }
            else
            {
                isPointIntensitySKip=false;
            }
            isSegmentEnd=isPointIntensitySKip | isPointDistanceSkip;
        }

        if( false==isSegmentEnd && i!=dotCount-1)
        {
            if(0==okPointCount)
            {
                tmp_firstPoint=tmp_p1;
                tmpPointListForFit.clear();
            }
            tmpPointListForFit.append(tmp_p2);
            okPointCount++;

        }
        else
        {
            miniSegmentQueue.clear();
            if(true==isPointChange)
            {
                tmp_endPoint=tmp_p1;
                segmentLength=sqrt((tmp_firstPoint.y-tmp_endPoint.y)*(tmp_firstPoint.y-tmp_endPoint.y)+
                                   (tmp_firstPoint.x-tmp_endPoint.x)*(tmp_firstPoint.x-tmp_endPoint.x));
                if(segmentLength<maxLength && segmentLength>minLength)
                {
                    goodSegmentCount++;
                    //去掉末尾2个可能异常的数据
                    tmpPointListForFit.pop_back();
                    tmpPointListForFit.pop_back();
                    tmpPointArry.append(tmpPointListForFit);
                    tmpPointListForFit.clear();
                    pointIndex.append(i);
//                    qDebug()<<"segmentLength"<<segmentLength;

                }
                qDebug()<<"this segmentLength"<<segmentLength;
                okPointCount=0;
                isPointChange=false;
            }

        }
    }



    //计算坐标系
    int segmentSize=tmpPointArry.size();
    MathPoint tmp_start,tmp_end;
    double coeralation;
    qDebug()<<"segmentSize"<<segmentSize;
    for(int i=0;i<segmentSize;i++)
    {
        double tmp_a,tmp_b,tmp_c;
        bool isLine=myFitLine(tmpPointArry[i],tmp_start,tmp_end,coeralation,tmp_a,tmp_b,tmp_c);
        qDebug()<<"segment line 直线相干系数"<<coeralation;
        if(true==isLine && fabs(coeralation)>standardCorelation)
        {
            lineStart_out.push_back(tmp_start);
            lineEnd_out.push_back(tmp_end);
        }
    }

    return 1;
}

int LineMatch::detectTwoReflectLine_withFitting(double minVAngle, double maxVAngle, double minLength,
                                                double maxLength,
                                              double allowMaxDeltaDistance, const LaserDataInfo &laserData,
                                    vector<MathPoint> &line1ListStart_out, vector<MathPoint> &line1ListEnd_out,
                                 vector<MathPoint> &line2ListStart_out, vector<MathPoint> &line2ListEnd_out,
                                 double &line1_corelation,double&line2_corelation,
                                 vector<double> &x_out, vector<double> &y_out, vector<double> &r_out,
                                                double &markAngleOut)
{
    if(isPrintVmarkProcessData)
    {
         qDebug()<<"1111111111111111111111111111111111111111111111111111LineMatch::detect2ReflectLine_withFitting begin";
    }
    x_out.clear();
    y_out.clear();
    r_out.clear();

    int dotCount=laserData.ranges.size();
    double deltaDistance=100000;

    MathPoint tmp_firstPoint;//直线段起始点
    MathPoint tmp_endPoint;//直线段结束点

    QQueue<MathPoint > miniSegmentQueue;//用于计算拐点
    //极端场合，偶尔运动时，会有0.4,　会有0.23.有距离跳点分割时0.15效果较好,出现0.14。二次校验出现0.008
    double idealLineCorelation=0.0005;

    double compareSegmentLength;//比较弦长
    compareSegmentLength=2*maxLength*sin(maxVAngle/2.0);


    for(int i=0;i<dotCount;i++)
    {
        //激光框选区域判断
        MathPoint validPoint;
        bool isIn=isUseThisLaserPoint(laserData,i,validPoint);
        if(false==isIn)
        {
            if(i==dotCount-1)
            {
                //最后一次做下封闭判断
                double tmp_angleFinded;
                MathPoint tmp_crossPointOut;
                if(true==isPointFittingVAngle(true,compareSegmentLength,minVAngle,maxVAngle,idealLineCorelation,
                                     miniSegmentQueue,line1_corelation,line2_corelation,
                                              tmp_angleFinded,tmp_crossPointOut,markAngleOut))
                {
                    x_out.push_back(tmp_crossPointOut.x);
                    y_out.push_back(tmp_crossPointOut.y);
                    r_out.push_back(tmp_crossPointOut.r);
                }
            }
            continue;
        }
        else
        {
            miniSegmentQueue.enqueue(validPoint);
        }

        //线段起始点标记
        if(1==miniSegmentQueue.size())
        {
            tmp_firstPoint=validPoint;
        }
        else
        {
            tmp_endPoint=validPoint;
        }


        //判断几何跳点
        bool isSegmentFinished=false;
        if(miniSegmentQueue.size()>=2)
        {
            deltaDistance=calculateDistance(miniSegmentQueue.last(),miniSegmentQueue[miniSegmentQueue.size()-2]);
            if(deltaDistance<allowMaxDeltaDistance)
            {
                isSegmentFinished=false;
            }
            else
            {
                isSegmentFinished=true;
                if(isPrintVmarkProcessData)
                {
                     qDebug()<<"发现跳点　isPointSkip=true deltaDistance="<<deltaDistance<<
                               " allowMaxDeltaDistance"<<allowMaxDeltaDistance;
                }

            }
        }


        //V角度判断，如果第一次成功找到后，就不继续找，可能丢失后面连续的线段，直到跳点。如果一直找，可能出现２个接近的结果。
        //找到一段成功的Ｖ槽后，清空前面的缓存点，防止找到第二个接近的Ｖ槽。
        double tmp_angleFinded;
        MathPoint tmp_crossPointOut;
        if(true==isPointFittingVAngle(isSegmentFinished,compareSegmentLength,minVAngle,maxVAngle,idealLineCorelation,
                             miniSegmentQueue,line1_corelation,line2_corelation,
                                      tmp_angleFinded,tmp_crossPointOut,markAngleOut))
        {
            isSegmentFinished=true;
            x_out.push_back(tmp_crossPointOut.x);
            y_out.push_back(tmp_crossPointOut.y);
            r_out.push_back(tmp_crossPointOut.r);
        }

        //线段切割后，数据重置
        if( true==isSegmentFinished )
        {

            miniSegmentQueue.clear();
            //打印线段长度
            double segmentLength=sqrt((tmp_firstPoint.y-tmp_endPoint.y)*(tmp_firstPoint.y-tmp_endPoint.y)+
                               (tmp_firstPoint.x-tmp_endPoint.x)*(tmp_firstPoint.x-tmp_endPoint.x));
            if(isPrintVmarkProcessData)
            {
                 qDebug()<<"比较线段结束，长度this segmentLength"<<segmentLength;
            }

        }
    }



    return 1;
}
//int LineMatch::detectTwoReflectLine_withFitting_old(double minVAngle, double maxVAngle, double minLength, double maxLength,
//                                              double allowMaxDeltaDistance, const LaserDataInfo &laserData,
//                                    vector<MathPoint> &line1ListStart_out, vector<MathPoint> &line1ListEnd_out,
//                                 vector<MathPoint> &line2ListStart_out, vector<MathPoint> &line2ListEnd_out,
//                                              double &line1_corelation,double&line2_corelation)
//{
//    if(isPrintVmarkProcessData)
//    {
//         qDebug()<<"1111111111111111111111111111111111111111111111111111LineMatch::detect2ReflectLine_withFitting begin";
//    }

//    bool isVAngleFinded=false;
//    int dotCount=laserData.ranges.size();
//    double deltaDistance=100000;
//    QQueue<MathPoint> tmp_pQueue;//样点 0好的旧点，１好的新点，２未知点
//    QQueue<MathPoint> servePoint;//前瞻观察点,2个观察点

//    MathPoint tmp_firstPoint;//直线段起始点
//    MathPoint tmp_endPoint;//直线段结束点
//    double segmentLength;//单段直线长度
//    QQueue<MathPoint > miniSegmentQueue;//用于计算拐点
//    //极端场合，偶尔运动时，会有0.4,　会有0.23.有距离跳点分割时0.15效果较好,出现0.14。二次校验出现0.008
//    double idealLineCorelation=0.0005;


//    int okPointCount=0;
//    bool isPointChange;//点的数据是否有变化


//    double compareSegmentLength;//比较弦长
//    compareSegmentLength=2*maxLength*sin(maxVAngle/2.0);
//    double angleForCompare_new=0;

//    for(int i=0;i<dotCount;i++)
//    {
//        if(i==dotCount-1)
//        {
////            qDebug()<<"last";
//        }
//        //激光框选区域判断
//        if(false==isLaserPointOk_laserFilter(laserData,i) && i!=dotCount-1)
//        {
//            continue;
//        }

//        MathPoint tmp_point;
//        tmp_point.x=laserData.ranges[i]*cos(laserData.angle_min+laserData.angle_increment*i);
//        tmp_point.y=laserData.ranges[i]*sin(laserData.angle_min+laserData.angle_increment*i);

//        if(false==isLaserPointOk_region(tmp_point))
//        {
////            qDebug()<<"nnnnnnnot in region";
//            continue;
//        }
////        qDebug()<<"iiiiiiiiiiiiiiiiiiiiiiiiiiii in region";

//        if( laserData.ranges[i]<=laserData.range_max)
//        {
//            servePoint.enqueue(tmp_point);
//            if(tmp_pQueue.size()<3)
//            {
//                tmp_pQueue.enqueue(servePoint.dequeue());
//                continue;
//            }
//            if(servePoint.size()<2)
//            {
//                continue;
//            }
//            //判断处理 2个异常点
//            double ds1=calculateDistance(tmp_pQueue.at(1),tmp_pQueue.at(2));
//            double ds2=calculateDistance(tmp_pQueue.at(2),servePoint.at(0));
//            double ds3=calculateDistance(servePoint.at(0),servePoint.at(1));
//            int tmp_big_distance_count=0;
//            if(ds1>allowMaxDeltaDistance )
//            {
//                tmp_big_distance_count++;
//            }
//            if(ds2>allowMaxDeltaDistance )
//            {
//                tmp_big_distance_count++;
//            }
//            if(ds3>allowMaxDeltaDistance )
//            {
//                tmp_big_distance_count++;
//            }
//            //取消异常点过滤
////            if(tmp_big_distance_count>1)
////            {
////                tmp_pQueue.pop_back();
////                tmp_pQueue.enqueue(servePoint.dequeue());
////                if(isPrintVmarkProcessData)
////                {
////                     qDebug()<<"0000发现过滤点，异常距离个数"<<tmp_big_distance_count;
////                }
////                continue;
////            }
////            else
////            {   tmp_pQueue.dequeue();
////                tmp_pQueue.enqueue(servePoint.dequeue());
////                miniSegmentQueue.enqueue(tmp_pQueue.at(1));

////            }
//            //不过滤异常点，ｓｉｃｋ雷达设置滤波后不需要了，过滤反而去掉了正常点。
//            tmp_pQueue.dequeue();
//            tmp_pQueue.enqueue(servePoint.dequeue());
//            miniSegmentQueue.enqueue(tmp_pQueue.at(1));

//            isPointChange=true;
//        }
//        else
//        {
//            continue;
//        }
//        //覆盖以前的点
//        if(0==okPointCount && 2==tmp_pQueue.size())
//        {
//            tmp_firstPoint=tmp_pQueue.at(1);
//        }

//        deltaDistance=calculateDistance(tmp_pQueue.at(0),tmp_pQueue.at(1));
//        bool isSegmentEnd;

//        //跳点判断，偶尔可能出现１～２个噪音飞点，滤波剔除噪音飞点
//        bool isPointDistanceSkip=false;
//        if(deltaDistance<allowMaxDeltaDistance)
//        {
//            isPointDistanceSkip=false;
//        }
//        else
//        {
//            isPointDistanceSkip=true;
//            if(isPrintVmarkProcessData)
//            {
//                 qDebug()<<"发现跳点　isPointSkip=true deltaDistance="<<deltaDistance<<" allowMaxDeltaDistance"<<allowMaxDeltaDistance;
//            }

//        }
//        //判断是否是激光数据最后一个点，最后一个点也需要做封闭处理
//        if(i==dotCount-1)
//        {
//            isPointDistanceSkip=true;
//            if(isPrintVmarkProcessData)
//            {
//                 qDebug()<<"激光数据的最后一个点";
//            }
//        }
//        if(true==isPointDistanceSkip)
//        {
//            isSegmentEnd=true;
//        }
//        else
//        {
//            isSegmentEnd=false;
//        }

//        //V角度判断，如果第一次成功找到后，就不继续找，可能丢失后面连续的线段，直到跳点。如果一直找，可能出现２个接近的结果。
//        if(false==isVAngleFinded)
//        {
//            double tmp_angleBias=0;
//            QQueue<MathPoint>  tmpPointListForFit_line1;//用于拟合
//            QQueue<MathPoint>  tmpPointListForFit_line2;//用于拟合
//            int tmpIsOk=detectLineAngleBias(isPointDistanceSkip,compareSegmentLength,M_PI-maxVAngle,
//                                            M_PI-minVAngle,idealLineCorelation,miniSegmentQueue,
//                                            tmpPointListForFit_line1,tmpPointListForFit_line2,
//                                            tmp_angleBias);
//            if(1==tmpIsOk)
//            {
//                isVAngleFinded=true;
//                if(isPrintVmarkProcessData)
//                {
//                     qDebug()<<"一次检测 vangle ="<<tmp_angleBias*180.0/M_PI;
//                }

//                MathPoint tmp_line1Start_out, tmp_line1End_out,tmp_line2Start_out,tmp_line2End_out;
//                int tmpResult=fittingLines(idealLineCorelation,tmpPointListForFit_line1,tmpPointListForFit_line2,
//                                                tmp_line1Start_out, tmp_line1End_out,tmp_line2Start_out,tmp_line2End_out,
//                                           line1_corelation,line2_corelation);
//                if(1!=tmpResult)
//                {
//                    if(isPrintVmarkProcessData)
//                    {
//                         qDebug()<<"二次校验　vmark 不合格 ";
//                    }

////                    return -1;
//                }
//                else
//                {
//                    line1ListStart_out.push_back(tmp_line1Start_out);
//                    line1ListEnd_out.push_back(tmp_line1End_out);
//                    line2ListStart_out.push_back(tmp_line2Start_out);
//                    line2ListEnd_out.push_back(tmp_line2End_out);
//                    if(isPrintVmarkProcessData)
//                    {
//                         qDebug()<<"二次校验　vmark 合格 ";
//                    }

//                }
//            }

//        }

//        if( false==isSegmentEnd && i!=dotCount-1)
//        {
//            if(0==okPointCount && 2==tmp_pQueue.size())
//            {
//                tmp_firstPoint=tmp_pQueue.at(1);
//            }

//            okPointCount++;

//        }
//        else
//        {
//            miniSegmentQueue.clear();
////            angleForCompare_old=0;
//            angleForCompare_new=0;
//            if(true==isPointChange)
//            {
//                tmp_endPoint=tmp_pQueue.at(0);
//                segmentLength=sqrt((tmp_firstPoint.y-tmp_endPoint.y)*(tmp_firstPoint.y-tmp_endPoint.y)+
//                                   (tmp_firstPoint.x-tmp_endPoint.x)*(tmp_firstPoint.x-tmp_endPoint.x));
//                if(isPrintVmarkProcessData)
//                {
//                     qDebug()<<"比较线段结束，长度this segmentLength"<<segmentLength;
//                }

//                okPointCount=0;
//                isPointChange=false;
//                if(true==isVAngleFinded)
//                {
//                    isVAngleFinded=false;
//                }

//            }
//            tmp_pQueue.clear();
//            servePoint.clear();

//        }
//    }


//    return 1;
//}

double LineMatch::calculateDistance(MathPoint point1, MathPoint point2)
{
    return sqrt((point1.y-point2.y)*(point1.y-point2.y)+ (point1.x-point2.x)*(point1.x-point2.x));
}

//int LineMatch::normalNoisePoint(int samplePointNum,int maxNoisePointNum,double noiseDistanceThreadhold,const LaserDataInfo &laserDataIn, LaserDataInfo &laserDataOut)
//{
//    //假设只有一个异常噪音点，如果和相邻的２个点的距离都大于设定值，则认为是噪音点
//    LaserDataInfo tmp_laserData=laserDataIn;
//    if(false==isLaserPointOk(laserData,i) && i!=dotCount-1)
//    {
//        continue;
//    }

//}


//int LineMatch::showDebugWindow(int id, const Mat &imageIn)
//{
////    if(true==safeMutex.tryLock())
////    {
////        displayLine(imageIn,detectedLines);
////        displayVMark(imageIn, x_detected, y_detected, r_detected);
////        safeMutex.unlock();
////    }
//    return 1;
//}

int LineMatch::setLineDetectPara(double rhoIn, double thetaIn, int thresholdIn, double minLineLengthIn, double maxLineGapIn)
{
    rho=rhoIn;
    theta=thetaIn;
    threshold_pt=thresholdIn;
    minLineLength=minLineLengthIn;
    maxLineGap=maxLineGapIn;
    return 1;
}



bool LineMatch::getCrossPoint_double(vector<double> firstLine, vector<double> secondLine, MathPoint &crossPoint,
                              double &crossDistance1, double &crossDistance2, double &angle_a, double &angle_b)
{
    double ka,kb;
    ka = (double)(firstLine[3] - firstLine[1]) / (double)(firstLine[2] - firstLine[0]); //求出LineA斜率
    kb = (double)(secondLine[3] - secondLine[1]) / (double)(secondLine[2] - secondLine[0]); //求出LineB斜率

    if(fabs(ka - kb)<0.000001)
    {
        return false;
    }

    crossPoint.x = (ka*firstLine[0] - firstLine[1] - kb*secondLine[0] + secondLine[1]) / (ka - kb);
    crossPoint.y = (ka*kb*(firstLine[0] - secondLine[0]) + ka*secondLine[1] - kb*firstLine[1]) / (ka - kb);

    //离交点最近的为起始点
    vector<double> tmpDistance;
    tmpDistance.resize(4);
    tmpDistance[0]=sqrt((crossPoint.x-firstLine[0])*(crossPoint.x-firstLine[0])+(crossPoint.y-firstLine[1])*(crossPoint.y-firstLine[1]));
    tmpDistance[1]=sqrt((crossPoint.x-firstLine[2])*(crossPoint.x-firstLine[2])+(crossPoint.y-firstLine[3])*(crossPoint.y-firstLine[3]));
    tmpDistance[2]=sqrt((crossPoint.x-secondLine[0])*(crossPoint.x-secondLine[0])+(crossPoint.y-secondLine[1])*(crossPoint.y-secondLine[1]));
    tmpDistance[3]=sqrt((crossPoint.x-secondLine[2])*(crossPoint.x-secondLine[2])+(crossPoint.y-secondLine[3])*(crossPoint.y-secondLine[3]));

    if(tmpDistance[0]>tmpDistance[1])
    {
        crossDistance1=tmpDistance[0];
        angle_a=atan2(firstLine[1]-crossPoint.y,firstLine[0]-crossPoint.x);
    }
    else
    {
        crossDistance1=tmpDistance[1];
        angle_a=atan2(firstLine[3]-crossPoint.y,firstLine[2]-crossPoint.x);
    }

    if(tmpDistance[2]>tmpDistance[3])
    {
        crossDistance2=tmpDistance[2];
        angle_b=atan2(secondLine[1]-crossPoint.y,secondLine[0]-crossPoint.x);
    }
    else
    {
        crossDistance2=tmpDistance[3];
        angle_b=atan2(secondLine[3]-crossPoint.y,secondLine[2]-crossPoint.x);
    }


    return true;
}

bool LineMatch::getCrossPoint_abc(double a1, double b1, double c1, double a2, double b2, double c2,
                                  MathPoint &crossPoint)
{
//    //忽略与坐标系完全平行的特殊情况
//    double k=(a2*b1-a1*b2);
//    if(fabs(k)<0.0001)
//    {
//        return -1;
//    }
//    crossPoint.y=(a1*c1-a2*c1)/k;
//    crossPoint.x=(-c1-b1*crossPoint.y)/a1;
    //与坐标系完全平行的特殊情况，下面的精度更高
    crossPoint.y=(a1*c2-a2*c1)/(a2*b1-a1*b2);
    crossPoint.x=(b1*c2-b2*c1)/(b2*a1-b1*a2);
    return true;
}

//void drawDetectedLines(cv::Mat& image, cv::Scalar color = cv::Scalar(255, 255, 255))
//{
//    std::vector&lt;cv::Vec4i&gt;::const_iterator it = lines.begin();
//    while (it!=lines.end())
//    {
//        cv::Point pt1((*it)[0], (*it)[1]);
//        cv::Point pt2((*it)[2], (*it)[3]);
//        cv::line(image, pt1, pt2, color);
//        it++;
//    }
//}
#ifdef D_USE_OPENCV
int LineMatch::detectLine(QString filePath)
{
    Mat src,transformedPicture;
//    src = imread("./1.pgm");
    src = imread(filePath.toStdString());
    processPicture(src,transformedPicture);

    vector<Vec4i> linesOut;
    detectLine(transformedPicture,linesOut );
    displayLine(src,linesOut);

    return linesOut.size();
}
#endif

int LineMatch::detectVMark_mapFile(QString filePath,double min_angle, double max_angle, double max_distance,
                                   double allow_vDistance, double allow_vAngle,
                                   vector<double> &x_out, vector<double> &y_out, vector<double> &r_out)
{

//    Mat src,transformedPicture;
////    src = imread("./1.pgm");
//    src = imread(filePath.toStdString());
//    processPicture(src,transformedPicture);
//    int tmpLineCount,tmpVmarkCount;
//    detectVMark_mat(transformedPicture,min_angle,max_angle,max_distance,allow_vDistance,allow_vAngle,x_out,y_out,r_out,
//                    tmpLineCount,tmpVmarkCount);

    return 1;

}

#ifdef D_USE_OPENCV
int LineMatch::detectVMark_mat(const Mat &imageIn, double min_angle, double max_angle, double max_distance,
                       double allow_vDistance, double allow_vAngle,
                               vector<double> &x_out, vector<double> &y_out, vector<double> &r_out,
                               int &detectedLineCountOut,int &detectedVmartCountOut)
{
    /*检查图像是否载入*/
    if (imageIn.empty())
    {
        printf("Error , LineMatch::detectVMark_mat...imageIn is empty\n");
        return -1;
    }
    safeMutex.lock();
    vector<Vec4i> linesOut;
    detectLine(imageIn,linesOut );
//    displayLine(imageIn,linesOut);
    detectedLineCountOut=linesOut.size();
    detectVMark(linesOut, min_angle, max_angle, max_distance, x_out, y_out, r_out);//todo 拟合直线
    detectedVmartCountOut=x_out.size();
    deleteTheSameVMark(allow_vDistance,allow_vAngle,x_out, y_out, r_out);
//    displayVMark(imageIn, x_out, y_out, r_out);

    //for debug
    detectedLines=linesOut;
    x_detected=x_out;
    y_detected=y_out;
    r_detected=r_out;
    safeMutex.unlock();
    return 1;
}
#endif

int LineMatch::processNoisePoint(double maxDistance, double a, double b, double c, QQueue<MathPoint>  &pointList)
{
    QQueue<MathPoint>  tmp_pointList=pointList;
    pointList.clear();
    int noisePointCount=0;
    int pointSize=tmp_pointList.size();
    for(int i=0;i<pointSize;i++)
    {
        double noiseDistance=getDistanceBetweenPointAndLine(a,b,c,tmp_pointList.at(i));
        if(noiseDistance>maxDistance)
        {
            qDebug()<<"noiseDistance="<<noiseDistance;
            noisePointCount++;
        }
        else
        {
            pointList.enqueue(tmp_pointList.at(i));
        }
    }

    return noisePointCount;
}

int LineMatch::fittingLines(double standardCorelation,QQueue<MathPoint>  tmpPointListForFit_line1,
                                 QQueue<MathPoint>  tmpPointListForFit_line2, MathPoint &line1Start_out,
                                 MathPoint &line1End_out,MathPoint &line2Start_out, MathPoint &line2End_out,
                            double &line1_corelation,double&line2_corelation)
{
    if(isPrintVmarkProcessData)
    {
        qDebug()<<"2次校验，line1点数="<<tmpPointListForFit_line1.size()
                  <<"，line2点数="<<tmpPointListForFit_line2.size();
    }


    //计算坐标系
    double maxErrorDistance=0.06;

    //line1 fit
    double tmp_a1,tmp_b1,tmp_c1;
    bool isLine=myFitLine(tmpPointListForFit_line1,line1Start_out,line1End_out,line1_corelation,
                          tmp_a1,tmp_b1,tmp_c1);
    if(isPrintVmarkProcessData)
    {
        qDebug()<<"line1 直线相干系数"<<line1_corelation;
    }

    if(true==isLine && fabs(line1_corelation)>standardCorelation)
    {
//        int tmp_noiseCount=processNoisePoint(maxErrorDistance,tmp_a,tmp_b,tmp_c,tmpPointListForFit_line1);
//        if(tmp_noiseCount>0)
//        {
//            qDebug()<<"warnning!!!!!!!!!!!!!!!!!!!!!!line1 have noise point tmp_noiseCount="<<tmp_noiseCount;
//            myFitLine(tmpPointListForFit_line1,line1Start_out,line1End_out,line1_corelation,tmp_a,tmp_b,tmp_c);
//        }

    }
    else
    {
        if(isPrintVmarkProcessData)
        {
            qDebug()<<"line1 isLine="<<isLine<<"line1_corelation="<<line1_corelation;
        }

        return -3;
    }

    //line2 fit
    double tmp_a2,tmp_b2,tmp_c2;
    isLine=myFitLine(tmpPointListForFit_line2,line2Start_out,line2End_out,line2_corelation,
                                             tmp_a2,tmp_b2,tmp_c2);
    if(isPrintVmarkProcessData)
    {
        qDebug()<<"line2 直线相干系数"<<line2_corelation;
    }

    if(true==isLine && fabs(line2_corelation)>standardCorelation)
    {
//        int tmp_noiseCount=processNoisePoint(maxErrorDistance,tmp_a,tmp_b,tmp_c,tmpPointListForFit_line2);
//        if(tmp_noiseCount>0)
//        {
//            qDebug()<<"warnning!!!!!!!!!!!!!!!!!!!!!!line2 have noise point tmp_noiseCount="<<tmp_noiseCount;
//            myFitLine(tmpPointListForFit_line2,line2Start_out,line2End_out,line2_corelation,tmp_a,tmp_b,tmp_c);
//        }
    }
    else
    {
        if(isPrintVmarkProcessData)
        {
            qDebug()<<"line2 isLine="<<isLine<<"line2_corelation="<<line2_corelation;
        }

        return -4;
    }


    return 1;

}

/*************************************************************************
 最小二乘法拟合直线，y = a*x + b; n组数据; r-相关系数[-1,1],fabs(r)->1,说明x,y之间线性关系好，
fabs(r)->0，x,y之间无线性关系，拟合无意义
 a_out*x+b_out*y+c_out=0
 对于异常样点非常敏感，所以需要提前判别删除异常样点
 a = (n*C - B*D) / (n*A - B*B)
 b = (A*D - B*C) / (n*A - B*B)
 r = E / F
 其中：
 A = sum(Xi * Xi)
 B = sum(Xi)
 C = sum(Xi * Yi)
 D = sum(Yi)
 E = sum((Xi - Xmean)*(Yi - Ymean))
 F = sqrt(sum((Xi - Xmean)*(Xi - Xmean))) * sqrt(sum((Yi - Ymean)*(Yi - Ymean)))

---------------------

本文来自 Coding小工 的CSDN 博客 ，全文地址请点击：https://blog.csdn.net/pl20140910/article/details/51926886?utm_source=copy

**************************************************************************/
bool LineMatch::lineFitLeastSquares(const QQueue<MathPoint> &points, MathPoint &point1Out,MathPoint &point2Out, double &corationCoefficient,
                         double &a_out,double &b_out,double &c_out)
{
//    float *data_x, float *data_y, , vector<float> &vResult
    int data_n=points.size();
    if(data_n < 2)
    {
        return false;
    }
    double A = 0.0;
    double B = 0.0;
    double C = 0.0;
    double D = 0.0;
    double E = 0.0;
    double F = 0.0;

    for (int i=0; i<data_n; i++)
    {
        A += points[i].x * points[i].x;
        B += points[i].x;
        C += points[i].x * points[i].y;
        D += points[i].y;
    }

    // 计算斜率a和截距b
    double a, b, temp = 0;
    if( temp = (data_n*A - B*B) )// 判断分母不为0
    {
        a = (data_n*C - B*D) / temp;
        b = (A*D - B*C) / temp;
    }
    else
    {
        a = 1;
        b = 0;
    }

    // 计算相关系数r
    double Xmean, Ymean;
    Xmean = B / data_n;
    Ymean = D / data_n;

    double tempSumXX = 0.0, tempSumYY = 0.0;
    for (int i=0; i<data_n; i++)
    {
        tempSumXX += (points[i].x - Xmean) * (points[i].x - Xmean);
        tempSumYY += (points[i].y - Ymean) * (points[i].y - Ymean);
        E += (points[i].x - Xmean) * (points[i].y - Ymean);
    }
    F = sqrt(tempSumXX) * sqrt(tempSumYY);

    double r;
    r = E / F;
//    qDebug()<<"直线相干系数"<<r;
    corationCoefficient=r;
    if(fabs(r)<0.001)
    {
        return false;
    }

    //如果直线轴与坐标轴接近平行，则下面的方法得到的可能不是起点。
//    point1Out.x=points[0].x;
//    point1Out.y=a*point1Out.x+b;

//    point2Out.x=points[data_n-1].x;
//    point2Out.y=a*point2Out.x+b;

    //新方法：根据斜率判断，使用ｘ带入，还是ｙ带入。
    if(fabs(a)<1)
    {
        point1Out.x=points[0].x;
        point1Out.y=a*point1Out.x+b;

        point2Out.x=points[data_n-1].x;
        point2Out.y=a*point2Out.x+b;
    }
    else
    {
        point1Out.y=points[0].y;
        point1Out.x=(point1Out.y-b)/a;

        point2Out.y=points[data_n-1].y;
        point2Out.x=(point2Out.y-b)/a;
    }

    a_out=a;
    b_out=-1.0;
    c_out=b;

    return true;
}

double LineMatch::getDistanceBetweenPointAndLine(double a,double b,double c,MathPoint point)
{
    double tmpValue1=fabs(a*point.x+b*point.y+c);
    double tmpValue2=sqrt(a*a+b*b);
    return tmpValue1/tmpValue2;
}
#define D_MAX_INTEGE 2147483647
bool LineMatch::isInRegion(double testx, double testy,std::vector<double> border_x, std::vector<double> border_y)
{
//    return false;//coredump for debug3,加上这一句后，还是会coredump，说明和下面没有关系
    //https://blog.csdn.net/u011722133/article/details/52813374

    //框选区域
    double tmpMinX=D_MAX_INTEGE;//-2^31
    double tmpMinY=D_MAX_INTEGE;
    double tmpMaxX=-D_MAX_INTEGE;
    double tmpMaxY=-D_MAX_INTEGE;
    double originMapWidth,originMapHeight;
    for(int i=0;i<border_x.size();i++)
    {
        if(border_x[i]<tmpMinX)
        {
           tmpMinX= border_x[i];
        }
        if(border_x[i]>tmpMaxX)
        {
           tmpMaxX= border_x[i];
        }
        if(border_y[i]<tmpMinY)
        {
           tmpMinY= border_y[i];
        }
        if(border_y[i]>tmpMaxY)
        {
           tmpMaxY= border_y[i];
        }
    }
    if (testx < tmpMinX || testx > tmpMaxX || testy < tmpMinY || testy > tmpMaxY)
    {
         return false;
    }
    //循环判断
    int nvert=border_x.size();
    int i, j, c = 0;
    for (i = 0, j = nvert-1; i < nvert; j = i++)
    {
        if ( ( (border_y[i]>testy) != (border_y[j]>testy) ) &&
            (testx < (border_x[j]-border_x[i]) * (testy-border_y[i]) / (border_y[j]-border_y[i]) + border_x[i]) )
        {
            c = !c;
        }

    }
    return c;




}

bool LineMatch::isUseThisLaserPoint(const LaserDataInfo &laserData,int index,MathPoint &pointOut)
{
    //激光框选区域判断
    if(false==isLaserPointOk_laserFilter(laserData,index) )
    {
        return false;
    }

    pointOut.x=laserData.ranges[index]*cos(laserData.angle_min+laserData.angle_increment*index);
    pointOut.y=laserData.ranges[index]*sin(laserData.angle_min+laserData.angle_increment*index);

    if(false==isLaserPointOk_region(pointOut))
    {
        return false;
    }

    return true;
}

bool LineMatch::isPointFittingVAngle(bool isSkipPoint,double compareLength, double idealAngle_min,
                                     double idealAngle_max,double allowCoration,
                                     const QQueue<MathPoint> miniSegmentQueueIn,
                                  double &line1_corelation,double&line2_corelation,
                                     double &angleFinded,MathPoint &crossPointOut,double &markAngleOut)
{

    QQueue<MathPoint>  tmpPointListForFit_line1;//用于拟合
    QQueue<MathPoint>  tmpPointListForFit_line2;//用于拟合
    int tmpIsOk=detectLineAngleBias(isSkipPoint,compareLength,M_PI-idealAngle_max,
                                    M_PI-idealAngle_min,allowCoration,miniSegmentQueueIn,
                                    tmpPointListForFit_line1,tmpPointListForFit_line2,
                                    angleFinded,crossPointOut,markAngleOut);
    if(1==tmpIsOk)
    {
       return true;
    }
    return false;

}



/**
  * 最小二乘法直线拟合（不是常见的一元线性回归算法）
  * 将离散点拟合为  a x + b y + c = 0 型直线
  * 假设每个点的 X Y 坐标的误差都是符合 0 均值的正态分布的。
  * 与一元线性回归算法的区别：一元线性回归算法假定 X 是无误差的，只有 Y 有误差。
  * 《机器视觉算法与应用（双语版）》[德] 斯蒂格（Steger C） 著；杨少荣 等 译 的书时写的。所有的公式推导都在书中 3.8.1
  */
//本文来自 liyuanbhu 的CSDN 博客 ，全文地址请点击：https://blog.csdn.net/liyuanbhu/article/details/51137038?utm_source=copy
bool LineMatch::lineFit_liyuanbhu(const QQueue<MathPoint> &points, MathPoint &point1Out,MathPoint &point2Out
                                  ,double &corationCoefficient, double &a_out,double &b_out,double &c_out)
{
    corationCoefficient=1.0;
    double a, b, c;
     int size = points.size();
     if(size < 2)
     {
         a = 0;
         b = 0;
         c = 0;
         return false;
     }
     double x_mean = 0;
     double y_mean = 0;

     for(int i = 0; i < size; i++)
     {
         x_mean += points[i].x;
         y_mean += points[i].y;
     }
     x_mean /= size;
     y_mean /= size; //至此，计算出了 x y 的均值

     double Dxx = 0, Dxy = 0, Dyy = 0;

     for(int i = 0; i < size; i++)
     {
         Dxx += (points[i].x - x_mean) * (points[i].x - x_mean);
         Dxy += (points[i].x - x_mean) * (points[i].y - y_mean);
         Dyy += (points[i].y - y_mean) * (points[i].y - y_mean);
     }

     double lambda = ( (Dxx + Dyy) - sqrt( (Dxx - Dyy) * (Dxx - Dyy) + 4 * Dxy * Dxy) ) / 2.0;
     double den = sqrt( Dxy * Dxy + (lambda - Dxx) * (lambda - Dxx) );

     if(fabs(den) < 1e-5)
     {
         if( fabs(Dxx / Dyy - 1) < 1e-5) //这时没有一个特殊的直线方向，无法拟合
         {
             return false;
         }
         else
         {
             a = 1;
             b = 0;
             c = - x_mean;
         }
     }
     else
     {
         a = Dxy / den;
         b = (lambda - Dxx) / den;
         c = - a * x_mean - b * y_mean;
     }

     //a=0的情况没有处理？

     //旧方法
//     point1Out.x=points[0].x;
//     point1Out.y=(-a*point1Out.x-c)/b;

//     point2Out.x=points[size-1].x;
//     point2Out.y=(-a*point2Out.x-c)/b;
     a_out=a;
     b_out=b;
     c_out=c;

     //新方法：根据斜率判断，使用ｘ带入，还是ｙ带入。
     int data_n=points.size();
     if(fabs(a/b)<1)
     {
          point1Out.x=points[0].x;
          point1Out.y=(-a*point1Out.x-c)/b;

          point2Out.x=points[size-1].x;
          point2Out.y=(-a*point2Out.x-c)/b;
     }
     else
     {
         point1Out.y=points[0].y;
         point1Out.x=-b*point1Out.y/a-c/a;

         point2Out.y=points[data_n-1].y;
         point2Out.x=-b*point2Out.y/a-c/a;
     }

     return true;
}




bool LineMatch::myFitLine(QQueue<MathPoint> pointData, MathPoint &point1Out, MathPoint &point2Out,
                          double &corationCoefficient, double &a_out,double &b_out,double &c_out)
{

    bool tmpResult;
    switch(lineFittingMethod)
    {
    case 0://线段与坐标轴接近平行的时候精度跳动太大
    {
            tmpResult = lineFitLeastSquares(pointData,point1Out,point2Out,corationCoefficient,a_out,b_out,c_out);
            break;
    }
    case 1://线段与坐标轴接近平行的时候，还能保持较好的精度
    {
            tmpResult = lineFit_liyuanbhu(pointData,point1Out,point2Out,corationCoefficient,a_out,b_out,c_out);
            break;
    }
    case 2:
    {
            tmpResult = opencvFitLine(pointData,point1Out,point2Out,corationCoefficient,a_out,b_out,c_out);
            break;
    }
    default:
    {
        return false;
    }
    }

    return tmpResult;
}

bool LineMatch::opencvFitLine(QQueue<MathPoint> pointData, MathPoint &point1Out, MathPoint &point2Out,
                              double &corationCoefficient, double &a_out, double &b_out, double &c_out)
{
//    corationCoefficient=1.0;
//    std::vector<cv::Point> points;
//    for(int i=0;i<pointData.size();i++)
//    {
//       points.push_back(Point(pointData[i].x*1000.0, pointData[i].y*1000.0));
//    }

//    //储存拟合直线的容器
//    Vec4f line;
//    //直线拟合函数
//    int method=CV_DIST_L12;
//    switch(method)
//    {
//    case CV_DIST_L1:
//    {
//        fitLine(points, line, CV_DIST_L1, 0, 1, 1);//运算５００ｍｓ,角度乱变
//        break;
//    }
//    case CV_DIST_L2:
//    {
//        fitLine(points, line, CV_DIST_L2, 0, 1, 1);//远近角度变换大
//        break;
//    }
//    case CV_DIST_L12:
//    {
//        fitLine(points, line, CV_DIST_L12, 0, 1, 1);//角度不稳定
//        break;
//    }
//    case CV_DIST_FAIR:
//    {
//        fitLine(points, line, CV_DIST_FAIR, 0, 0.01, 0.01);
//        break;
//    }
//    case CV_DIST_WELSCH:
//    {
//        fitLine(points, line, CV_DIST_WELSCH, 0, 0.01, 0.01);
//        break;
//    }
//    case CV_DIST_HUBER:
//    {
//        fitLine(points, line, CV_DIST_HUBER, 0, 0.01, 0.01);
//        break;
//    }
//    }

////     a_out*x+b_out*y+c_out=0
//    corationCoefficient=1.0;

//    point1Out.x=line[0]/1000.0;
//    point1Out.y=line[1]/1000.0;
//    point2Out.x=line[2]/1000.0;
//    point2Out.y=line[3]/1000.0;

////    angle=atan2(line[3]-line[1],line[2]-line[0]);
////    qDebug()<< "CV_DIST_HUBER line " <<line[0]<<" "<<line[1]<<" "<<line[2]<<" "<<line[3]<<" "
////            <<" angle="<<angle*180/M_PI;

    return true;

}

bool LineMatch::isBreakPoint(const QQueue<MathPoint> miniSegmentQueueIn,double standardCoralation)
{

    if(miniSegmentQueueIn.size()<D_SEGMENT_MIN_SIZE)
    {
        return false;
    }
    MathPoint point1Out,point2Out;
    double corationCoefficient=0;
    double tmp_a,tmp_b,tmp_c;
    bool tmpResult = myFitLine(miniSegmentQueueIn,point1Out,point2Out,corationCoefficient,tmp_a,tmp_b,tmp_c);
//    qDebug()<<"isBreakPoint 直线相干系数"<<corationCoefficient;
    if(false==tmpResult || fabs(corationCoefficient)<standardCoralation)
    {
        qDebug()<<"isBreakPoint new 直线相干系数"<<corationCoefficient<<" standardCoralation="<<standardCoralation
               <<" tmpResult="<<tmpResult;
        return true;
    }
    return false;

}

bool LineMatch::getFitLineAngleBias(const QQueue<MathPoint> miniSegmentQueueIn,double allowCoration, double &angleOut)
{
    if(miniSegmentQueueIn.size()<D_SEGMENT_MIN_SIZE)
    {
        return false;
    }
    QQueue<MathPoint> tmp_SegmentQueue=miniSegmentQueueIn;
    //线段１
    QQueue<MathPoint> tmp_points1;
    for(int i=0;i<D_SEGMENT_MIN_SIZE/2;i++)
    {
        tmp_points1.enqueue(tmp_SegmentQueue.dequeue());
    }
    MathPoint point1Outa,point2Outa;
    double corationCoefficienta;
    double tmp_a,tmp_b,tmp_c;
    bool isLine = myFitLine(tmp_points1,point1Outa,point2Outa,corationCoefficienta,tmp_a,tmp_b,tmp_c);
    if(fabs(corationCoefficienta)<allowCoration || false==isLine )
    {
        return false;
    }
    //    qDebug()<<"isBreakPoint 直线相干系数"<<corationCoefficient;
    double angleOut1=atan2(point2Outa.y-point1Outa.y,point2Outa.x-point1Outa.x);

    //线段２
    QQueue<MathPoint> tmp_points2;
    for(int i=D_SEGMENT_MIN_SIZE/2;i<D_SEGMENT_MIN_SIZE;i++)
    {
        tmp_points2.enqueue(tmp_SegmentQueue.dequeue());
    }
    MathPoint point1Outb,point2Outb;
    double corationCoefficientb;
    isLine = myFitLine(tmp_points2,point1Outb,point2Outb,corationCoefficientb,tmp_a,tmp_b,tmp_c);
    if(fabs(corationCoefficientb)<allowCoration || false==isLine )
    {
        return false;
    }
    //    qDebug()<<"isBreakPoint 直线相干系数"<<corationCoefficient;
    double angleOut2=atan2(point2Outb.y-point1Outb.y,point2Outb.x-point1Outb.x);

    angleOut=fabs(angleRotationJudgement(angleOut1,angleOut2));
    return true;
}

int LineMatch::detectLineAngleBias(bool isSkipPoint,double compareLength, double idealAngle_min,
                                   double idealAngle_max,double allowCoration,
                                   const QQueue<MathPoint> miniSegmentQueueIn,
                                   QQueue<MathPoint> &firstSegmentQueueOut
                                   , QQueue<MathPoint> &secondSegmentQueueOut, double &angleFinded,
                                    MathPoint &crossPointOut,double &markAngleOut)
{
    static double angleForCompare_new=0;
    static double angleForCompare_old=0;
    static double angleForCompare_oldold=0;
    static double angleForCompare_old3=0;
    static double angleForCompare_old4=0;
    static MathPoint crossPoint_old ,crossPoint_oldold ;//交叉点(含角度）
    static double markAngle_old=0;//V槽夹角１３０度
    QQueue<MathPoint> tmp_SegmentQueue=miniSegmentQueueIn;

    //提前终止的情况处理
    if(true==isSkipPoint)
    {
        angleForCompare_old3=0;
        angleForCompare_old4=0;
        angleForCompare_oldold=0;
        angleForCompare_old=0;
        //情况０：（上一次探测的角度在范围内）
        if(angleForCompare_new>idealAngle_min && angleForCompare_new<idealAngle_max)
        {
            int validPointSize=tmp_SegmentQueue.size();
            for(int i=0;i<validPointSize/2;i++)
            {
                firstSegmentQueueOut.enqueue(tmp_SegmentQueue.dequeue());
            }
            for(int i=validPointSize/2;i<validPointSize;i++)
            {
                secondSegmentQueueOut.enqueue(tmp_SegmentQueue.dequeue());
            }

            markAngleOut=markAngle_old;
            angleFinded=angleForCompare_new;
            crossPointOut=crossPoint_old;

            if(isPrintVmarkProcessData)
            {
                 qDebug()<<"情况０: 提前终止 sucess. angleForCompare_new="<<angleForCompare_new;
            }
            angleForCompare_new=0;
            return 1;

        }
        else
        {
            if(isPrintVmarkProcessData)
            {
                 qDebug()<<"true==isSkipPoint 提前终止 角度不符合要求　angleForCompare_new="<<angleForCompare_new;
            }
            angleForCompare_new=0;
            return -1;
        }
    }


    //正常处理流程
    bool isOver=false;
    double tmpLine1Length;
    bool isPoped=false;
    while(false==isOver)
    {
        if(tmp_SegmentQueue.size()>2)
        {
            tmpLine1Length=sqrt((tmp_SegmentQueue.first().y-tmp_SegmentQueue.last().y)
                                *(tmp_SegmentQueue.first().y-tmp_SegmentQueue.last().y)+
                                (tmp_SegmentQueue.first().x-tmp_SegmentQueue.last().x)
                                *(tmp_SegmentQueue.first().x-tmp_SegmentQueue.last().x));
            if(isPrintVmarkProcessData)
            {
//                qDebug()<<"tmpLine1Length= "<<tmpLine1Length<<" compareLength= "<<compareLength;
            }
        }
        else
        {
            isOver=true;
//            qDebug()<<"error,,,,,tmp_SegmentQueue.size()<=2";
            return -1;
        }


        if(tmpLine1Length<compareLength/2)
        {
//            qDebug()<<"error,,,,,tmpLine1Length<compareLength/2";
            return -2;
        }
        else if(tmpLine1Length>compareLength)
        {
            //标准Ｖ槽对于ＳＩＣＫ TIM雷达，２米范围内效果可以。
            tmp_SegmentQueue.pop_front();//雷达和Ｖ槽距离较远时，点数据稀疏，会导致比较点太少，线段１和线段２的切分不均匀，最终角度误差非常大。
            isPoped=true;
        }
        else
        {
            isOver=true;
        }

    }
    if(false==isPoped)
    {
//        qDebug()<<"error,,,,,no poped";
        return -3;
    }

    int validPointSize=tmp_SegmentQueue.size();
    //线段１
    for(int i=0;i<validPointSize/2;i++)
    {
        firstSegmentQueueOut.enqueue(tmp_SegmentQueue.dequeue());
    }
    MathPoint point1Outa,point2Outa;
    double corationCoefficienta=0;
    double tmp_a1,tmp_b1,tmp_c1;
    bool isLine = myFitLine(firstSegmentQueueOut,point1Outa,point2Outa,corationCoefficienta
                            ,tmp_a1,tmp_b1,tmp_c1);
    if(fabs(corationCoefficienta)<allowCoration || false==isLine )
    {
        if(isPrintVmarkProcessData)
        {
            qDebug()<<"error,,,,,line1直线系数不合格 "
                   <<corationCoefficienta<<" allowCoration"<<allowCoration;
        }
        return -4;
    }
    //    qDebug()<<"isBreakPoint 直线相干系数"<<corationCoefficient;
    double angleOut1=atan2(point2Outa.y-point1Outa.y,point2Outa.x-point1Outa.x);

    //线段２
    double tmp_a2,tmp_b2,tmp_c2;
    for(int i=validPointSize/2;i<validPointSize;i++)
    {
        secondSegmentQueueOut.enqueue(tmp_SegmentQueue.dequeue());
    }
    MathPoint point1Outb,point2Outb;
    double corationCoefficientb=0;

    isLine = myFitLine(secondSegmentQueueOut,point1Outb,point2Outb,corationCoefficientb
                       ,tmp_a2,tmp_b2,tmp_c2);
    if(fabs(corationCoefficientb)<allowCoration || false==isLine )
    {
        if(isPrintVmarkProcessData)
        {
        qDebug()<<"error,,,,,line2直线系数不合格 "
               <<corationCoefficientb<<" allowCoration"<<allowCoration;
        }
        return -5;
    }
    //    qDebug()<<"isBreakPoint 直线相干系数"<<corationCoefficient;
    double angleOut2=atan2(point2Outb.y-point1Outb.y,point2Outb.x-point1Outb.x);

    angleForCompare_old4=angleForCompare_old3;
    angleForCompare_old3=angleForCompare_oldold;
    angleForCompare_oldold=angleForCompare_old;
    angleForCompare_old=angleForCompare_new;
    angleForCompare_new=angleRotationJudgement(angleOut2,angleOut1);//不能加绝对值，因为会错误识别凸角

    //求交点
    getCrossPoint_abc(tmp_a1,tmp_b1,tmp_c1,tmp_a2,tmp_b2,tmp_c2,crossPointOut);
    //求角度
    double angle_a=atan2(point1Outa.y-point2Outa.y,point1Outa.x-point2Outa.x);
    double angle_b=atan2(point2Outb.y-point1Outb.y,point2Outb.x-point1Outb.x);
    markAngle_old=angleRotationJudgement(angle_a,angle_b);
    crossPointOut.r=angle_a-markAngle_old/2;



    if(isPrintVmarkProcessData)
    {
          qDebug()<<"探测角度angleForCompare_new="<<angleForCompare_new*180.0/M_PI
                 <<" ,idealMin="<<idealAngle_min*180.0/M_PI<<" ,idealMax="<<idealAngle_max*180.0/M_PI
                  <<" ,crossPointOut.r "<<crossPointOut.r*180.0/M_PI ;
    }





    if(laser_angleIncrement>0.3/180*3.14)//tim561做雷达内部滤波
    {
        //情况１：（范围）小>（范围）大>（范围）小
        //0.5m距离lms151会错误提前识别。
        if( (angleForCompare_oldold>idealAngle_min && angleForCompare_oldold<idealAngle_max)
                &&(angleForCompare_old>idealAngle_min && angleForCompare_old<idealAngle_max)
                && (angleForCompare_new>idealAngle_min && angleForCompare_new<idealAngle_max))
        {

            if( angleForCompare_new<angleForCompare_old
                 && angleForCompare_old<angleForCompare_oldold)
            {
                angleFinded=angleForCompare_old;
                markAngleOut=markAngle_old;
                crossPointOut=crossPoint_old;
                if(isPrintVmarkProcessData)
                {
                      qDebug()<<"探测情况１";
                }
                angleForCompare_new=0;
                return 1;
            }
        }
        //情况２a：（范围）小>（范围）大>小（超过负范围）
        if(angleForCompare_new<idealAngle_min)
        {
            if( (angleForCompare_old>idealAngle_min && angleForCompare_old<idealAngle_max)
                 &&(angleForCompare_oldold>idealAngle_min && angleForCompare_oldold<idealAngle_max)
                 && angleForCompare_old>angleForCompare_oldold)
            {
                markAngleOut=markAngle_old;
                angleFinded=angleForCompare_old;
                crossPointOut=crossPoint_old;
                if(isPrintVmarkProcessData)
                {
                      qDebug()<<"探测情况２a";
                }
                angleForCompare_new=0;
                return 1;
            }
        }

        //情况２b：（范围）小>（范围）大>　大（超过正范围）
        if(angleForCompare_new>idealAngle_max)
        {
            if( (angleForCompare_old>idealAngle_min && angleForCompare_old<idealAngle_max)
                 &&(angleForCompare_oldold>idealAngle_min && angleForCompare_oldold<idealAngle_max)
                 && angleForCompare_old>angleForCompare_oldold)
            {
                markAngleOut=markAngle_old;
                angleFinded=angleForCompare_old;
                crossPointOut=crossPoint_old;
                if(isPrintVmarkProcessData)
                {
                      qDebug()<<"探测情况２b";
                }
                angleForCompare_new=0;
                return 1;
            }
        }


        //情况２c：（范围）小>（范围）大>　偏差大于４度（０．０６９ｒａｄ)
        if(fabs(angleForCompare_new-angleForCompare_old)>0.069)
        {
            if( (angleForCompare_old>idealAngle_min && angleForCompare_old<idealAngle_max)
                 &&(angleForCompare_oldold>idealAngle_min && angleForCompare_oldold<idealAngle_max)
                 && angleForCompare_old>angleForCompare_oldold)
            {
                markAngleOut=markAngle_old;
                angleFinded=angleForCompare_old;
                crossPointOut=crossPoint_old;
                if(isPrintVmarkProcessData)
                {
                      qDebug()<<"探测情况２c";
                }
                angleForCompare_new=0;
                return 1;
            }
        }
        //情况3：（范围）大>（范围）小>（范围）小
        if( (angleForCompare_oldold>idealAngle_min && angleForCompare_oldold<idealAngle_max)
                &&(angleForCompare_old>idealAngle_min && angleForCompare_old<idealAngle_max)
                && (angleForCompare_new>idealAngle_min && angleForCompare_new<idealAngle_max) )
        {
            if( angleForCompare_new<angleForCompare_old
                 && angleForCompare_old<angleForCompare_oldold)
            {
                markAngleOut=markAngle_old;
                angleFinded=angleForCompare_oldold;
                crossPointOut=crossPoint_oldold;
                if(isPrintVmarkProcessData)
                {
                      qDebug()<<"探测情况３";
                }
                angleForCompare_new=0;
                return 1;
            }
        }
        //情况4：（范围）大>小（超过最小范围）0.5m距离lms151会错误提前识别。
        if(angleForCompare_new<idealAngle_min)
        {
            if( (angleForCompare_old>idealAngle_min && angleForCompare_old<idealAngle_max))
            {
                markAngleOut=markAngle_old;
                angleFinded=angleForCompare_old;
                crossPointOut=crossPoint_old;
                if(isPrintVmarkProcessData)
                {
                      qDebug()<<"探测情况4";
                }
                angleForCompare_new=0;
                return 1;
            }
        }

        //情况5：（范围）大>小（超过最大范围）
        if(angleForCompare_new>idealAngle_max)
        {
            if( (angleForCompare_old>idealAngle_min && angleForCompare_old<idealAngle_max))
            {
                markAngleOut=markAngle_old;
                angleFinded=angleForCompare_old;
                crossPointOut=crossPoint_old;
                if(isPrintVmarkProcessData)
                {
                      qDebug()<<"探测情况5";
                }
                angleForCompare_new=0;
                return 1;
            }
        }

        //情况6：（范围）大>　偏差大于４度（０．０６９ｒａｄ)
        if(fabs(angleForCompare_new-angleForCompare_old)>0.069)
        {
            if( (angleForCompare_old>idealAngle_min && angleForCompare_old<idealAngle_max))
            {
                markAngleOut=markAngle_old;
                angleFinded=angleForCompare_old;
                crossPointOut=crossPoint_old;
                if(isPrintVmarkProcessData)
                {
                      qDebug()<<"探测情况6";
                }
                angleForCompare_new=0;
                return 1;
            }
        }
    }
    else
    {
        //情况１：变大，变大，变小，变小。 lms151不做滤波，会导致角度波动较为复杂。
        //0.5m距离lms151会错误提前识别。
        if( (angleForCompare_oldold>idealAngle_min && angleForCompare_oldold<idealAngle_max)
                &&(angleForCompare_old>idealAngle_min && angleForCompare_old<idealAngle_max)
                && (angleForCompare_new>idealAngle_min && angleForCompare_new<idealAngle_max)
                && (angleForCompare_old3>idealAngle_min && angleForCompare_old3<idealAngle_max)
                && (angleForCompare_old4>idealAngle_min && angleForCompare_old4<idealAngle_max))
        {

            if( angleForCompare_new<angleForCompare_old
                 && angleForCompare_old<angleForCompare_oldold
                    && angleForCompare_oldold>angleForCompare_old3)
            {
                angleFinded=angleForCompare_old;
                markAngleOut=markAngle_old;
                crossPointOut=crossPoint_old;
                if(isPrintVmarkProcessData)
                {
                      qDebug()<<"探测情况１plus lms0.25degree ";
                }
                angleForCompare_new=0;
                return 1;
            }

            double allowBias=1.0/360.0*3.1415;
//            qDebug()<<"777777"<<fabs(angleForCompare_old-angleForCompare_oldold)<<allowBias;
            if( angleForCompare_new<angleForCompare_old
                 && angleForCompare_old>angleForCompare_oldold
                 && fabs(angleForCompare_new-angleForCompare_old)<allowBias
                 && fabs(angleForCompare_old-angleForCompare_oldold)<allowBias)
            {
                angleFinded=angleForCompare_old;
                markAngleOut=markAngle_old;
                crossPointOut=crossPoint_old;
                if(isPrintVmarkProcessData)
                {
                      qDebug()<<"探测情况2plus lms0.25degree allowBias"<<allowBias;
                }
                angleForCompare_new=0;
                return 1;
            }
        }


    }


    //保持
    crossPoint_oldold=crossPoint_old;
    crossPoint_old=crossPointOut;

    return -1;
}



//int LineMatch::processPicture(const Mat &originPicture, Mat &transformedPicture)
//{
//    Mat grayImg, binImg;
//    /*检查图像是否载入*/
//    if (originPicture.empty()) {
//        printf("Error Loading Image...\n");
//        return -1;
//    }

//    /*转为灰度图*/
//    if (originPicture.channels() == 3)
//    {
//        cvtColor(originPicture, grayImg, CV_BGR2GRAY);
//    }
//    else if (originPicture.channels() == 2)
//    {
//        grayImg = originPicture.clone();
//    }
////    imshow("灰度图 image", grayImg);

//    /*二值化*/
//    threshold(grayImg, binImg, 100, 255, THRESH_BINARY);
////    //adaptiveThreshold(grayImg, binImg, 255, ADAPTIVE_THRESH_MEAN_C, THRESH_BINARY, 15, -2);
////    imshow("binary image", binImg);


//    /*腐蚀*/
//    Mat element = getStructuringElement(MORPH_RECT, Size(2, 1));
//    Mat erodeImg;
//    erode(binImg, erodeImg, element);
////    imshow("erode", erodeImg);

//    transformedPicture=~erodeImg;
//    //transformedPicture=erodeImg;
//    imshow("blackBackGround", transformedPicture);
//    return 1;
//}

//int LineMatch::detectLine( const Mat &imagineSource, vector<Vec4i> &linesOut)
//{
//    /*霍夫直线检测*/
//    HoughLinesP(imagineSource, linesOut, rho, theta, threshold_pt, minLineLength, maxLineGap);
////    HoughLines(imagineSource, linesOut, 1, CV_PI/360, 10);

////    CV_EXPORTS_W void HoughLines( InputArray image, OutputArray lines,
////                                  double rho, double theta, int threshold,
////                                  double srn=0, double stn=0 );
////    CV_EXPORTS_W void HoughLinesP( InputArray image, OutputArray lines,
////                                   double rho, double theta, int threshold,
////                                   double minLineLength=0, double maxLineGap=0 );
//}

//int LineMatch::detectVMark(const vector<Vec4i> &linesIn, double min_angle, double max_angle,double max_distance,
//                           vector<double> &x_out,vector<double> &y_out,vector<double> &r_out)
//{
//    x_out.clear();
//    y_out.clear();
//    r_out.clear();
//    double tmpX,tmpY,tmpR;
//    int tmpLineCount=linesIn.size();//?为什么陷入ｉ死循环，调试查看linesIn=０ｉｔｅｍ
//    for(int i=0;i<tmpLineCount-1;i++)
//    {
//        for(int j=i+1;j<tmpLineCount;j++)
//        {
//            bool isOk=isVMark(linesIn[i],linesIn[j],min_angle,max_angle,max_distance,tmpX,tmpY,tmpR);
//            if(true==isOk)
//            {
//                //todo 保存直线，后面拟合
//                x_out.push_back(tmpX);
//                y_out.push_back(tmpY);
//                r_out.push_back(tmpR);
//            }
//        }

//    }
//    return 1;
//}

int LineMatch::detectStructLine(vector<double> p1_x, vector<double> p1_y, vector<double> p1_r,
                                vector<double> p2_x, vector<double> p2_y, vector<double> p2_r,
                                double min_gap, double max_gap, double minAngle, double maxAngle,
                                vector<double> &x_out, vector<double> &y_out, vector<double> &r_out,
                                vector<double> &gapDistanceOut,vector<double> &angleBiasOut)
{
    x_out.clear();
    y_out.clear();
    r_out.clear();
    int p1_size=p1_x.size();
    int p2_size=p2_x.size();
    for(int i=0;i<p1_size;i++)
    {
        for(int j=0;j<p2_size;j++)
        {
            double tmp_gapDistance,tmp_angleBias;
            int isOk=isStructLine(p1_x[i],p1_y[i],p1_r[i],p2_x[j],p2_y[j],p2_r[j],min_gap,max_gap,minAngle,maxAngle,
                                  tmp_gapDistance,tmp_angleBias);
            gapDistanceOut.push_back(tmp_gapDistance);
            angleBiasOut.push_back(tmp_angleBias);
            if(1==isOk)
            {
                x_out.push_back((p1_x[i]+p2_x[j])/2.0);
                y_out.push_back((p1_y[i]+p2_y[j])/2.0);
                r_out.push_back(atan2(p2_y[j]-p1_y[i],p2_x[j]-p1_x[i]));
            }
        }

    }
    return 1;
}

int LineMatch::detectStructLine_vmark(vector<MathPoint> line1Start,vector<MathPoint>  line1End,
                                      vector<MathPoint> line2Start, vector<MathPoint>  line2End,
                                      double min_gap, double max_gap, double minAngle, double maxAngle,
                                      vector<double> &x_out, vector<double> &y_out, vector<double> &r_out,
                                      vector<double> &gapDistanceOut, vector<double> &angleBiasOut)
{
    x_out.clear();
    y_out.clear();
    r_out.clear();
    int p1_size=line1Start.size();
    int p2_size=line2Start.size();
    double tmp_x,tmp_y,tmp_r;
    for(int i=0;i<p1_size;i++)
    {
        for(int j=0;j<p2_size;j++)
        {
            double tmp_gapDistance,tmp_angleBias;
            int isOk=isVMark_structLine(line1Start[i],line1End[i],line2Start[j],line2End[j],
                                        min_gap,max_gap,minAngle,maxAngle,
                                  tmp_gapDistance,tmp_angleBias,tmp_x,tmp_y,tmp_r);
            gapDistanceOut.push_back(tmp_gapDistance);
            angleBiasOut.push_back(tmp_angleBias);
            if(1==isOk)//不判断焦点距离和夹角了。　　　必须要判断，会有错误数据。
            {
                x_out.push_back(tmp_x);
                y_out.push_back(tmp_y);
                r_out.push_back(tmp_r);
            }
        }

    }
    return 1;
}

int LineMatch::isStructLine(double p1_x, double p1_y, double p1_r, double p2_x, double p2_y, double p2_r,
                            double min_gap, double max_gap, double minAngle, double maxAngle,double &gapDistanceOut,double &angleBiasOut)
{
    gapDistanceOut=sqrt((p1_x-p2_x)*(p1_x-p2_x)+(p1_y-p2_y)*(p1_y-p2_y));
    angleBiasOut=angleRotationJudgement(p1_r,p2_r);
//    qDebug()<<"gapDistance="<<gapDistance<<" angleBias"<<fabs(angleBias);
    if(gapDistanceOut<max_gap && gapDistanceOut>min_gap && fabs(angleBiasOut)<maxAngle && fabs(angleBiasOut)>minAngle)
    {

        return 1;
    }

    return -1;
}

//bool LineMatch::isVMark(Vec4i firstLine, Vec4i secondLine, double min_angle, double max_angle,
//                        double max_distance, double &x_out,double &y_out, double &r_out)
//{
//    //焦点距离最小值必须小于给定值

//    MathPoint crossPoint;
//    double crossDistance1,crossDistance2;
//    double angle_a, angle_b;

//    vector<double> tmp_firstLine,tmp_secondLine;
//    tmp_firstLine.resize(4);
//    tmp_secondLine.resize(4);
//    tmp_firstLine[0]=firstLine[0];
//    tmp_firstLine[1]=firstLine[1];
//    tmp_firstLine[2]=firstLine[2];
//    tmp_firstLine[3]=firstLine[3];
//    tmp_secondLine[0]=secondLine[0];
//    tmp_secondLine[1]=secondLine[1];
//    tmp_secondLine[2]=secondLine[2];
//    tmp_secondLine[3]=secondLine[3];

////    bool isOk=getCrossPoint(firstLine,secondLine,crossPoint,crossDistance1,crossDistance2,angle_a, angle_b);
//    bool isOk=getCrossPoint_double(tmp_firstLine,tmp_secondLine,crossPoint,crossDistance1,crossDistance2,angle_a, angle_b);
//    if(false==isOk)
//    {
//        return false;
//    }

//    if(crossDistance1>max_distance || crossDistance2>max_distance )
//    {
//        return false;
//    }

//    //判断夹角
//    double angle=angleRotationJudgement(angle_a,angle_b);
//    if(fabs(angle)<min_angle || fabs(angle)>max_angle)
//    {
//        return false;
//    }

//    x_out=crossPoint.x;
//    y_out=crossPoint.y;
//    r_out=angle_a-angle/2;
//    return true;

//}

bool LineMatch::calculateCrossPoint(MathPoint line1Start, MathPoint line1End,
                                    MathPoint line2Start, MathPoint line2End,
                                    double &x_out, double &y_out, double &r_out)
{
    vector<double> firstLine, secondLine;
    firstLine.resize(4);
    firstLine[0]=line1Start.x;
    firstLine[1]=line1Start.y;
    firstLine[2]=line1End.x;
    firstLine[3]=line1End.y;
    secondLine.resize(4);
    secondLine[0]=line2Start.x;
    secondLine[1]=line2Start.y;
    secondLine[2]=line2End.x;
    secondLine[3]=line2End.y;

    //焦点距离最小值必须小于给定值
    MathPoint crossPoint;
    double crossDistance1,crossDistance2;
    double angle_a, angle_b;

    bool isOk=getCrossPoint_double(firstLine,secondLine,crossPoint,crossDistance1,crossDistance2,angle_a, angle_b);
    if(isPrintVmarkProcessData)
    {
         qDebug()<<"三次判断　crossDistance1"<<crossDistance1<<" crossDistance2"<<crossDistance2;
    }

    if(false==isOk)
    {
        qDebug()<<"三次判断　no crossPoint ";
        return false;
    }


    //判断夹角
    double angleBiasOut=angleRotationJudgement(angle_a,angle_b);//?


    x_out=crossPoint.x;
    y_out=crossPoint.y;
    r_out=angle_a-angleBiasOut/2;
    return true;
}

bool LineMatch::isVMark_structLine(MathPoint line1Start,MathPoint  line1End,
                                   MathPoint line2Start,MathPoint  line2End,
                                   double min_gap, double max_gap, double minAngle, double maxAngle,
                                   double &gapDistanceOut, double &angleBiasOut, double &x_out, double &y_out, double &r_out)
{
    vector<double> firstLine, secondLine;
    firstLine.resize(4);
    firstLine[0]=line1Start.x;
    firstLine[1]=line1Start.y;
    firstLine[2]=line1End.x;
    firstLine[3]=line1End.y;
    secondLine.resize(4);
    secondLine[0]=line2Start.x;
    secondLine[1]=line2Start.y;
    secondLine[2]=line2End.x;
    secondLine[3]=line2End.y;

    //焦点距离最小值必须小于给定值
    MathPoint crossPoint;
    double crossDistance1,crossDistance2;
    double angle_a, angle_b;

    bool isOk=getCrossPoint_double(firstLine,secondLine,crossPoint,crossDistance1,crossDistance2,angle_a, angle_b);
    if(isPrintVmarkProcessData)
    {
         qDebug()<<"三次判断　crossDistance1"<<crossDistance1<<" crossDistance2"<<crossDistance2;
    }

    if(false==isOk)
    {
        qDebug()<<"三次判断　no crossPoint ";
        return false;
    }
    if(crossDistance1>crossDistance2)
    {
       gapDistanceOut= crossDistance1;
    }
    else
    {
        gapDistanceOut= crossDistance2;
    }

    if(crossDistance1>max_gap || crossDistance2>max_gap )
    {
        if(isPrintVmarkProcessData)
        {
             qDebug()<<"三次判断　crossDistance not good";
        }

        return false;
    }

    //判断夹角
    angleBiasOut=angleRotationJudgement(angle_a,angle_b);
    if(isPrintVmarkProcessData)
    {
         qDebug()<<"三次判断　angleBiasOut"<<angleBiasOut*180.0/M_PI;
    }

    if(fabs(angleBiasOut)<minAngle || fabs(angleBiasOut)>maxAngle)
    {
        if(isPrintVmarkProcessData)
        {
             qDebug()<<"三次判断　angleBiasOut bad minAngle="<<minAngle*180.0/M_PI<<" maxAngle"<<maxAngle*180.0/M_PI;
        }

        return false;
    }

    x_out=crossPoint.x;
    y_out=crossPoint.y;
    r_out=angle_a-angleBiasOut/2;
    return true;
}

//int LineMatch::displayVMark(const Mat &originPicture, vector<double> &x_out, vector<double> &y_out, vector<double> &r_out)
//{
//    if( originPicture.empty())
//    {
//        return -1;
//    }
//    Mat result = originPicture.clone();
//    double tmpLength=100;

//    for (int i = 0; i < x_out.size(); i++)
//    {
//        circle(result, Point(x_out[i], y_out[i]), 20,Scalar(150, 255, 0), 2,8);
//        line(result, Point(x_out[i], y_out[i]), Point(x_out[i]+tmpLength*cos(r_out[i]), y_out[i]+tmpLength*sin(r_out[i])),
//             Scalar(150, 255, 0), 2, 8);
//    }

//    QString showMsg="vmarkSize=";
//    showMsg+=QString::number(x_out.size());
//    putText(result,showMsg.toStdString(),Point(10,15),CV_FONT_HERSHEY_SIMPLEX,0.5,Scalar(255,100,0),2,2);

//    imshow("displayVMark", result);
//    return 1;
//}

//int LineMatch::displayLine(const Mat &originPicture, vector<Vec4i> &linesIn)
//{
//    if( originPicture.empty()  )
//    {
//        return -1;
//    }
//    Mat result = originPicture.clone();

//    for (int i = 0; i < linesIn.size(); i++)
//    {
//        line(result, Point(linesIn[i][0], linesIn[i][1]), Point(linesIn[i][2], linesIn[i][3]), Scalar(200, 255, 50), 2, 8);
//    }
//    QString showMsg="lineSize=";
//    showMsg+=QString::number(linesIn.size());
//    putText(result,showMsg.toStdString(),Point(10,15),CV_FONT_HERSHEY_SIMPLEX,0.5,Scalar(255,100,0),2,2);

//    imshow("displayLine", result);
//    return 1;
//}

int LineMatch::deleteTheSameVMark(double allowDistance, double allowAngle, vector<double> &x_out,
                                  vector<double> &y_out, vector<double> &r_out)
{
    vector<double> tmp_x,tmp_y, tmp_r;
    if(x_out.size()>=1)
    {
        tmp_x.push_back(x_out[0]);
        tmp_y.push_back(y_out[0]);
        tmp_r.push_back(r_out[0]);
    }

    for(int i=0;i<x_out.size();i++)
    {
        int tmpTargetCount=tmp_x.size();
        int isSameCount=0;
        for(int j=0;j<tmpTargetCount;j++)
        {
            bool isOk=isTheSameVMark(allowDistance,allowAngle,x_out[i],y_out[i],r_out[i]
                                     ,tmp_x[j],tmp_y[j],tmp_r[j]);
            if(true==isOk)
            {
                break;
            }
            else
            {
                isSameCount++;
            }
        }
        if(isSameCount==tmpTargetCount)
        {
            tmp_x.push_back(x_out[i]);
            tmp_y.push_back(y_out[i]);
            tmp_r.push_back(r_out[i]);
        }

    }
    x_out=tmp_x;
    y_out=tmp_y;
    r_out=tmp_r;
    return 1;
}

bool LineMatch::isTheSameVMark(double allowDistance, double allowAngle, double x_1, double y_1, double r_1,
                               double x_2, double y_2, double r_2)
{
    double tmpDistance=  sqrt((x_1-x_2)*(x_1-x_2)+(y_1-y_2)*(y_1-y_2));
    double tmpAngle=angleRotationJudgement(r_1,r_2);

    if(tmpDistance<allowDistance &&  fabs(tmpAngle)<allowAngle)
    {
        return true;
    }
    else
    {
        return false;
    }

    return false;

}


double LineMatch::angleRotationJudgement(double targetAngle, double initialAngle)
{
    double angle1,angle2,angle3;
    angle1=targetAngle-initialAngle;
    angle2=angle1+2*M_PI;
    angle3=angle1-2*M_PI;
    double minNum;
    if(fabs(angle1)<=fabs(angle2))
    {
        minNum=angle1;
    }
    else
    {
        minNum=angle2;
    }

    if(fabs(angle3)<=fabs(minNum))
    {
        minNum=angle3;
    }

    return minNum;

}

bool LineMatch::isLaserPointOk_laserFilter(const LaserDataInfo &laserData,int index)
{
    double tmpAngle=laserData.angle_min+index*laserData.angle_increment;
    if(laserData.intensities.size()<=index || 0>index)
    {
        return false;
    }
    if(laserData.intensities[index]<minumIntensity|| laserData.ranges[index]>maxLaserRange
            || tmpAngle<minAngle || tmpAngle> maxAngle)
    {
        return false;
    }
    //判断是否在有效区域

    return true;
}

bool LineMatch::isLaserPointOk_region(const MathPoint testPoint)
{
    return isInRegion(testPoint.x,testPoint.y,laserRegion_x,laserRegion_y);
}
